"""
描述：三轴微型吊舱测试代码
作者：无烬
邮箱：xueyuankui.good@163.com
"""

#!/usr/bin/env python3
import os
import sys
import time
import json
import socket
import datetime
import threading
import signal

import cv2

TASK_ID_SET_GIMBAL_STEP  = 6
TASK_ID_SET_GIMBAL_ANGLE = 13
TASK_ID_SET_GIMBAL_RATE  = 14
class GimbalPod(object):
    """
    The class for communicate with pod and control it
    """
    def __init__(self):
        ###
        self.pitch = 0.0
        self.yaw   = 0.0
        ### 
        self.pod_ip = "192.168.0.152"

        ### camera
        self.frame      = None
        self.th_camera_receive    = threading.Thread(target=self.th_camera_receive_fun)
        self.th_camera_receive.setDaemon(True)
        self.flag_camera_receive  = False

        ### gimbal
        self.th_gimbal_com_receive    = threading.Thread(target=self.th_gimbal_com_receive_fun)
        self.th_gimbal_com_receive.setDaemon(True)
        self.flag_gimbal_com_receive  = False

        self.setup()
    def setup(self):
        self.camera_com = "rtsp://" + self.pod_ip + "/live"
        self.camera_cap           = cv2.VideoCapture(self.camera_com)
        self.gimbal_connection  = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.gimbal_server_ip   = self.pod_ip
        self.gimbal_server_port = 8083
        try:
            self.gimbal_connection.connect((self.gimbal_server_ip, self.gimbal_server_port))
        except Exception as e:
            print("ERROR: gimbal com connection failed")

    ###
    def run(self):
        self.flag_camera_receive     = True
        self.flag_gimbal_com_receive = True
        self.th_camera_receive.start()
        self.th_gimbal_com_receive.start()

    def stop(self):
        self.flag_camera_receive     = False
        self.flag_gimbal_com_receive = False
        self.gimbal_connection.close()
        self.camera_cap.release()
        cv2.destroyAllWindows()
    def __del__(self):
        print("__del__")
        self.stop()

    ############################################################
    ### gimbal
    ############################################################
    def th_gimbal_com_receive_fun(self):
        while self.flag_gimbal_com_receive:
            recv_data = self.gimbal_connection.recv(1024)
            #print(recv_data)
            str_list = recv_data.decode().split()
            for str_data in str_list:
                if len(str_data) == 0:
                    continue
                try:
                    data_json = json.loads(str_data.encode())
                    #print(data_json)
                    if data_json['task'] == 0:
                        if 'gimbal_pitch' in data_json:
                            self.pitch = data_json['gimbal_pitch']
                        if 'gimbal_yaw' in data_json:
                            self.yaw   = data_json['gimbal_yaw']
                        print('INFO: gimbal pitch:{}, yaw:{}'.format(self.pitch, self.yaw))
                    if data_json['task'] == TASK_ID_SET_GIMBAL_RATE:
                        print(recv_data)
                except Exception as e:
                    print("ERROR: ", e)
                    print("ERROR: recv_data=", recv_data)
                    print("ERROR: data_json=", data_json)

    def set_gimbal_pose_pitch_rate(self, rate=0):
        """
        set pitch rate
        rate: type int, unit deg/s, -200~+200, >0 turn up, <0 turn down
        """
        data_json = {"task":TASK_ID_SET_GIMBAL_RATE,
                     "speed":rate,
                     "isPitch":True
                    }
        data_send = json.dumps(data_json).encode()
        try:
            self.gimbal_connection.send(data_send)
        except Exception as e:
            print("ERROR: send failed {}".format(e))
        time.sleep(1)

    def set_gimbal_pose_yaw_rate(self, rate=0):
        """
        set yaw rate
        rate: type int, unit deg/s, -200~+200, >0 turn right, <0 turn left
        """
        data_json = {"task":TASK_ID_SET_GIMBAL_RATE,
                     "speed":rate,
                     "isPitch":False
                    }
        data_send = json.dumps(data_json).encode()
        try:
            self.gimbal_connection.send(data_send)
        except Exception as e:
            print("ERROR: send failed {}".format(e))
        time.sleep(1)

    def set_gimbal_pose_pitch_angle(self, angle=0):
        """
        set pitch angle
        rate: type int, unit deg/s, -90~+15, >0 turn up, <0 turn down
        """
        data_json = {"task":TASK_ID_SET_GIMBAL_ANGLE,
                     "angle":angle,
                     "isPitch":True
                    }
        data_send = json.dumps(data_json).encode()
        try:
            self.gimbal_connection.send(data_send)
        except Exception as e:
            print("ERROR: send failed {}".format(e))
        time.sleep(1)

    def set_gimbal_pose_yaw_angle(self, angle=0):
        """
        set yaw angle
        rate: type int, unit deg, -90~+90, >0 turn right, <0 turn left
        """
        data_json = {"task":TASK_ID_SET_GIMBAL_ANGLE,
                     "angle":angle,
                     "isPitch":False
                    }
        data_send = json.dumps(data_json).encode()
        try:
            self.gimbal_connection.send(data_send)
        except Exception as e:
            print("ERROR: send failed {}".format(e))
        time.sleep(1)

    def set_gimbal_pose_pitch_step(self, angle=0):
        """
        control gimbal pitch to move a step by given angle
        angle: unit 0.1deg
        """
        direction = 1 if angle < 0 else 0
        data_json = {"task":TASK_ID_SET_GIMBAL_STEP,
                     "pitch":direction,
                     "angle":abs(angle)
                    }
        data_send = json.dumps(data_json).encode()
        try:
            self.gimbal_connection.send(data_send)
        except Exception as e:
            print("ERROR: send failed {}".format(e))
        time.sleep(1)

    def set_gimbal_pose_yaw_step(self, angle=0):
        """
        control gimbal yaw to move a step by given angle
        angle: unit 0.1deg
        """
        direction = 1 if angle > 0 else 0
        data_json = {"task":TASK_ID_SET_GIMBAL_STEP,
                     "yaw":direction,
                     "angle":abs(angle)
                    }
        data_send = json.dumps(data_json).encode()
        try:
            self.gimbal_connection.send(data_send)
        except Exception as e:
            print("ERROR: send failed {}".format(e))
        time.sleep(1)

    def gimbal_reset(self):
        data_json = {"task":TASK_ID_SET_GIMBAL_STEP,
                     "reset":True
                    }
        data_send = json.dumps(data_json).encode()
        try:
            self.gimbal_connection.send(data_send)
        except Exception as e:
            print("ERROR: send failed {}".format(e))
        time.sleep(1)

    ############################################################
    ### camera
    ############################################################
    def th_camera_receive_fun(self):
        while self.flag_camera_receive:
            ret, self.frame = self.camera_cap.read()
            if not ret:
                pass
            else:
                height = self.frame.shape[0]
                width  = self.frame.shape[1]
                #self.frame = cv2.resize(self.frame, (int(width/3), int(height/3)))

                cv2.imshow('pod', self.frame)

                key = cv2.waitKey(1)
                if key == ord('q'):
                    break

   
def test_set_angle():
    ## set angle test: ok
    print('set gimbal pitch angle')
    pod.set_gimbal_pose_pitch_angle(15)
    time.sleep(5)
    pod.set_gimbal_pose_pitch_angle(-60)
    time.sleep(5)
    print('set gimbal yaw angle')
    pod.set_gimbal_pose_yaw_angle(-50)
    time.sleep(5)
    pod.set_gimbal_pose_yaw_angle(50)
    time.sleep(5)
    ### reset test: ok
    print('reset')
    pod.gimbal_reset()

def test_set_angle_rate():
    ### set angle rate test: ok
    print('set gimbal pitch rate')
    pod.set_gimbal_pose_pitch_rate(15)
    time.sleep(5)
    pod.set_gimbal_pose_pitch_rate(-15)
    time.sleep(5)
    print('set gimbal yaw rate')
    pod.set_gimbal_pose_yaw_rate(15)
    time.sleep(5)
    pod.set_gimbal_pose_yaw_rate(-15)
    time.sleep(5)
    ### reset test: ok
    print('reset')
    pod.gimbal_reset()

def test_set_angle_step():
    ### set step test:ok
    print('set gimbal pitch step')
    pod.set_gimbal_pose_pitch_step(15)
    time.sleep(5)
    pod.set_gimbal_pose_pitch_step(-20)
    time.sleep(5)
    print('set gimbal yaw step')
    pod.set_gimbal_pose_yaw_step(30)
    time.sleep(5)
    pod.set_gimbal_pose_yaw_step(-30)
    time.sleep(5)
    ### reset test: ok
    print('reset')
    pod.gimbal_reset()

if __name__ == '__main__':
    pod = GimbalPod()
    pod.run()
    #test_set_angle()
    #test_set_angle_step()
    test_set_angle_rate()
    # while True:
    #     pass
    #     time.sleep(1)


